//#define EIGEN_NO_STATIC_ASSERT
//#define EIGEN_NO_DEBUG
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>

using namespace std;
using namespace Eigen;


static void demoQR()
{
    Matrix3f A;
    Vector3f b;
    A << 1,2,3,  4,5,6,  7,8,10;
    b << 3, 3, 4;


    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the vector b:\n" << b << endl;

    // QR 分解
    Vector3f x = A.colPivHouseholderQr().solve(b);
    cout << "The solution is:\n" << x << endl;

    // QR 分解结果
    HouseholderQR<Matrix3f> qr(A);
    cout << "QR:\n" << qr.matrixQR() << endl;
    Matrix3f R = qr.matrixQR().triangularView<Upper>();
    Matrix3f Q = qr.householderQ();
    cout << "R:\n" << R << endl;
    cout << "Q:\n" << Q << endl;
    cout << "Q * R = \n" << Q * R << endl;
}

static void demoLdlt()
{
    Matrix2f A, b;
    A << 2, -1, -1, 3;
    b << 1, 2, 3, 1;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the right hand side b:\n" << b << endl;
    Matrix2f x = A.ldlt().solve(b);
    cout << "The solution is:\n" << x << endl;

    // 特征值
    // 实对称矩阵可以保证对角化成功
    SelfAdjointEigenSolver<Matrix2f> eigen_solver(A);
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;
}

/// 特征值和特征向量
static void demoAdjoint()
{

   Matrix2f A;
   A << 1, 2, 2, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
   if (eigensolver.info() != Success) abort();
   cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
   cout << "Here's a matrix whose columns are eigenvectors of A \n"
        << "corresponding to these eigenvalues:\n"
        << eigensolver.eigenvectors() << endl;
   cout<<"A*vec(1) = \n"<<A*(eigensolver.eigenvectors().col(0))<<endl;
   cout<<"e(1)*vec(1) = \n"<<eigensolver.eigenvalues()(0)*eigensolver.eigenvectors().col(0) << endl;

   // $H = USV^T$
   Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
   cout << "U:\n" << svd.matrixU() << endl;
   cout << "V:\n" << svd.matrixV() << endl;
   cout << "S:\n" << svd.singularValues() << endl;

}

static void demoLU()
{
    Matrix2f A;
    A << 1, 2, 2, 3;
    cout << "Here is the matrix A:\n" << A << endl;

    // LU
    cout << "---LU---\n";
    PartialPivLU<Matrix2f> lu(A);
    cout << "LU" << lu.matrixLU() << endl;

    Matrix2f L = Matrix2f::Identity();
    L(1,0) = lu.matrixLU()(1,0);
    Matrix2f U = lu.matrixLU();
    U(1,0) = 0;
    cout << "L:\n" << L << endl;
    cout << "U:\n" << U << endl;

    cout << "L*U:\n" << L*U << endl;
}

const char * usage =
" cmd <option>"
"   1 - demoQR  colPivHouseholderQr\n"
"   2 - demoLdlt LDLT\n"
"   3 - demoAdjoint 计算特征值和特征向量\n"
"   4 - demoLU\n"
;

static void help()
{
    printf("%s\n", usage);
}

int main(int argc, char *argv[])
{

    int option = 4;
    if (argc >= 2) {
        option = atoi(argv[1]);
    }

    switch (option) {
    case 1:
        demoQR();
        break;
    case 2:
        demoLdlt();
        break;
    case 3:
        demoAdjoint();
        break;
    case 4:
        demoLU();
        break;
    default:
        help();
        break;
    }

    return 0;
}
